#include <Servo.h>  //引入舵机的库文件
Servo head_servo;  //创建舵机实例控制一个舵机
#include <Wire.h> 
float distance = 0;  //距离数据十进制值
float ds[3];  //创建一个浮点数的数组，用于储存读取到的值
int distance1; //存放测距值的变量1
int distance2; //存放测距值的变量2
int distance3; //存放测距值的变量3

#define INA 7  //定义引脚控制左边电机方向
#define ENA 6  //控制左边电机速度
#define INB 8  //右边电机方向
#define ENB 5  //右边电机速度

void setup() {
  Serial.begin(9600);
  Wire.begin();
  pinMode(INA, OUTPUT);  //设置控制电机的引脚为输出模式
  pinMode(ENA, OUTPUT);
  pinMode(INB, OUTPUT);
  pinMode(ENB, OUTPUT);
  head_servo.attach(12,500,2500);  //设置舵机的引脚为A2，脉冲范围是500~2500ms
  head_servo.write(90);  //90度
  delay(200);
}

void loop() {
  func_avoid(); //执行避障功能的子函数
}

void func_avoid()
{
  distance1 = checkdistance(); //获取超声波函数返回的值
  Serial.print("distance1 = ");
  Serial.println(distance1);  //换行打印出超声波传感器测到的值
  if(distance1 > 15)
  {
    car_forward();
  }
  else
  {
    car_stop();
    head_servo.write(180);  //小蜗牛头转到左边
    delay(300);
    distance2 = checkdistance(); //获取左边的距离值
    delay(100);
    head_servo.write(10);   //小蜗牛头转到右边
    delay(500);
    distance3 = checkdistance(); //获取右边的距离值
    delay(100);
    if(distance2 > distance3)
    {
      car_left();  //左转
      head_servo.write(90);
      delay(300);  //延时时间大小决定转动的角度大小
    }
    else
    {
      car_right();
      head_servo.write(90);
      delay(300);
    }
  }
}

//超声波测距函数
float checkdistance() {
  char i = 0;
  ds[0]=0;
  ds[1]=0;
  ds[2]=0;
  Wire.beginTransmission(0x57);   //地址为0X57 写8位数据为AE，读8位数据位AF
  Wire.write(1);                  //写命令0X01,0X01为开始测量命令 
  Wire.endTransmission();            
  delay(100);                     //测量周期延时，一个周期为100mS,设置120MS,留余量    
  Wire.requestFrom(0x57,3);       //地址为0X57 读3个8位距离数据       
  while (Wire.available())
  {
   ds[i++] = Wire.read();
  }          
  distance=(ds[0]*65536+ds[1]*256+ds[2])/10000; //计算成CM值     
  Serial.print("distance : "); 
  if ((1<=distance)&&(600>=distance))           //1CM-6M之间数值显示 
  {
  Serial.print(distance);
   Serial.print(" CM ");  
  }
  else 
  {
  Serial.print(" - - - - ");                   //无效数值数值显示 - - - - 
  }
  Serial.println();    
  delay(10);                                  //调节测量周期  
  return distance;
}

//小蜗牛前进
void car_forward()
{
  digitalWrite(INA, HIGH);  //输出高电平，控制左边电机正转
  analogWrite(ENA, 100);    //PWM输出，控制左边电机速度
  digitalWrite(INB, HIGH);  //右边电机正转
  analogWrite(ENB, 100);    //右边电机速度
}
//小蜗牛后退
void car_back()
{
  digitalWrite(INA, LOW);  //左边反转
  analogWrite(ENA, 100);   //左边速度
  digitalWrite(INB, LOW);  //右边反转
  analogWrite(ENB, 100);   //右边速度
}
//小蜗牛左旋转
void car_left()
{
  digitalWrite(INA, LOW);
  analogWrite(ENA, 200);
  digitalWrite(INB, HIGH);
  analogWrite(ENB, 200);
}
//小蜗牛右旋转
void car_right()
{
  digitalWrite(INA, HIGH);
  analogWrite(ENA, 200);
  digitalWrite(INB, LOW);
  analogWrite(ENB, 200);
}
//停止
void car_stop()
{
  digitalWrite(INA, HIGH);
  analogWrite(ENA, 0);
  digitalWrite(INB, HIGH);
  analogWrite(ENB, 0);
}
